#include "CQue.h"
#include <commlib.h>

static int cque_push(CQue* thes, const char* v, unsigned short v_size) {
    int i = 0;
    if(!thes->q_buf_size || !thes->q_buf || v_size <= 0 || !v) {
        return PUSH_FAILED;
    }
    if(thes->used_bytes + v_size + 2 > thes->q_buf_size) { //要不push成功,要么失败,不要发送部分
        return PUSH_FAILED;
    }

	if (thes->is_lock_when_write) {
		while(SEM_WAIT_FAILED == thes->lock.wait(&thes->lock));
	}

	//write size byte by byte in little-endian
    thes->q_buf[thes->w_idx] = (v_size & 0xFF);
    ++thes->w_idx;
    if(thes->w_idx >= thes->q_buf_size) {
        thes->w_idx = 0;
    }
    
    thes->q_buf[thes->w_idx] = (v_size >> 8);
    ++thes->w_idx;
    if(thes->w_idx >= thes->q_buf_size) {
        thes->w_idx = 0;
    }

    //write buf data
    for(; i < v_size; ++i) {        
        thes->q_buf[thes->w_idx] = v[i];
        ++thes->w_idx;
        if(thes->w_idx >= thes->q_buf_size) {
            thes->w_idx = 0;
        }
    }

    thes->msg_cnt++;
    thes->used_bytes += (v_size + 2);

	if (thes->is_lock_when_write) {
		thes->lock.signal(&thes->lock);
	}
	return PUSH_OK;
}

static int cque_pop(CQue* thes, char* out_v, unsigned short* v_size) { //out_v buf size? TODO
	unsigned char l, h;
    int i = 0;
    if (thes->msg_cnt <= 0) {
		return POP_FAILED;
	}
    
    l = thes->q_buf[thes->r_idx];
    thes->r_idx++;
    if(thes->r_idx >= thes->q_buf_size) {
        thes->r_idx = 0;
    }

    h = thes->q_buf[thes->r_idx];
    thes->r_idx++;
    if(thes->r_idx >= thes->q_buf_size) {
        thes->r_idx = 0;
    }
    *v_size = ((h << 8) | l);
    
    for(; i < *v_size; ++i) {
        out_v[i] = thes->q_buf[thes->r_idx];
        thes->r_idx++;
        if(thes->r_idx >= thes->q_buf_size) {
            thes->r_idx = 0;
        }
    }
    thes->msg_cnt--;
	return POP_OK;
}

int cque_set_queue_buf(CQue* thes, char* buf, int siz) {
    if(thes->q_buf || thes->q_buf_size || !siz || !buf) {
        return -1;
    }
    thes->q_buf = buf;
    thes->q_buf_size = siz;
    return 0;
}

void construct_queue(CQue* thes, int with_lock) {
	memset(thes, 0, sizeof(*thes));
	thes->is_lock_when_write = with_lock;
	if (with_lock) {
		construct_sem(&thes->lock, 1);
	} else {
		memset(&thes->lock, 0, sizeof(thes->lock));
	}

	thes->push = cque_push;
	thes->pop = cque_pop;
    thes->set_queue_buf = cque_set_queue_buf;
}
